# -*- coding: cp1252 -*-


class Transfer :
    def __init__(self,
                 minimum, # position minimum servo
                 maximum, # position maximum servo
                 angle1,
                 position1,
                 angle2,
                 position2,
                 ) :
        self._minimum   = minimum
        self._maximum   = maximum

        a1  = float(angle1)
        p1  = float(position1)
        a2  = float(angle2)
        p2  = float(position2)

        # position = a . angle + b
        self._a = (p1 - p2) / (a1-a2)
        self._b = (a1*p2 - a2*p1) / (a1-a2)

    def limited_position( self, position ) :
        return max( self._minimum, min( self._maximum, position ) )
    
    def angle_to_position( self, angle, limit = True ) :
        position = int( self._a * angle + self._b)
        if limit :
            return self.limited_position( position )
        else :
            return position

    def position_to_angle( self, position ) :
        return (position - self._b) / self._a

    def __call__(self, angle) :
        return self.angle_to_position( angle, limit = True )

    
class Arm :
    def __init__( self, scc32, cynematic ) :
        self._scc32 = scc32
        self._cynematic = cynematic
        
        self._base_canal           = 0
        self._shoulder_canal       = (16, (17, 35) ) # canal1, (canal2, offset)
        self._elbow_canal          = 2
        self._wrist_canal          = 3
        self._grip_canal           = 4
        self._wrist_rotate_canal   = 5        

        # get current position from scc32 board
        self._base         = scc32.get_position( self._base_canal )
        self._shoulder     = scc32.get_position( self._shoulder_canal[0] )
        self._elbow        = scc32.get_position( self._elbow_canal )
        self._wrist        = scc32.get_position( self._wrist_canal )
        self._grip         = scc32.get_position( self._grip_canal )
        self._wrist_rotate = scc32.get_position( self._wrist_rotate_canal )
         
        self._base_tr         = Transfer(555, 2311, -88, 555, 90, 2311 )
        self._shoulder_tr     = Transfer(500, 2166, -90, 500, 90, 2500 )
        self._elbow_tr        = Transfer(888, 2100,  90, 500,  0, 1500 )
        self._wrist_tr        = Transfer(588, 2411, -90, 588, 90, 2411 )
        self._grip_tr         = Transfer(850, 2000,  30, 850,  0, 1923 )
        self._wrist_rotate_tr = Transfer(500, 2500, -90, 500, 90, 2311 )

    def base( self, angle, delay = None ) :
        print("BASE %s" % angle)
        pos = self._base_tr( angle )
        
        self._scc32.send1( self._base_canal,
                           pos,
                           delay )

    def shoulder( self, angle, delay = None ) :
        print("SHOULDER %s" % angle)
        pos = self._shoulder_tr( angle )
        
        self._scc32.send( { self._shoulder_canal[0] : pos,
                            self._shoulder_canal[1][0] :
                               pos + self._shoulder_canal[1][1]
                            },
                           delay )

    def elbow( self, angle, delay = None ) :
        print("ELBOW %s" % angle)
        pos = self._elbow_tr( angle )
        
        self._scc32.send1( self._elbow_canal,
                           pos,
                           delay )

    def wrist( self, angle, delay = None ) :
        print("wrist %s" % angle)
        pos = self._wrist_tr( angle )
        
        self._scc32.send1( self._wrist_canal,
                           pos,
                           delay )

    def grip( self, angle, delay = None ) :
        print("grip %s" % angle)
        pos = self._grip_tr( angle )
        
        self._scc32.send1( self._grip_canal,
                           pos,
                           delay )

    def wrist_rotate( self, angle, delay = None ) :
        print("wrist_rotate %s" % angle)
        pos = self._wrist_rotate_tr( angle )
        
        self._scc32.send1( self._wrist_rotate_canal,
                           pos,
                           delay )


    def park( self ) :
        self.grip(30,1000)
        input()
        self.wrist_rotate(0,1000)
        input()
        self.wrist(0,1000)
        input()
        self.elbow(-60,4000)
        input()
        self.shoulder(65,1000)

    def goto( self, x,y,z, wrist = -90, delay = 1000 ) :
        try :
            base, shoulder, elbow, wrist =  self._cynematic.xyz_to_angles(x,y,z,wrist)
        except ValueError :
            return
        shoulder_pos = self._shoulder_tr( shoulder)
        self._scc32.send( {self._base_canal : self._base_tr( base ),
                           self._shoulder_canal[0] : shoulder_pos,
                           self._shoulder_canal[1][0] :
                               shoulder_pos + self._shoulder_canal[1][1],
                           self._elbow_canal : self._elbow_tr( elbow ),
                           self._wrist_canal : self._wrist_tr( wrist ),
                           self._wrist_rotate_canal : self._wrist_rotate_tr(-base) },
                           delay )
        

